#include "motion.h"
#include "algorithm.h"
#include "motor.h"
#include "oled_hardware_i2c.h"
#include "stdio.h"
#include "stdlib.h"
#include "uart.h"

#define TAG "Motion"

#define L 16.0f

static int16_t current_ls = 0, current_rs = 0;
extern pid_control_t hh;

void limitSpeed(int16_t *left_speed, int16_t *right_speed, int16_t max_speed) {
  // 确保最大速度为正值
  int16_t abs_max = abs(max_speed);
  if (abs_max == 0)
    return; // 避免除以零

  // 保存原始速度的符号
  int16_t left_sign = (*left_speed >= 0) ? 1 : -1;
  int16_t right_sign = (*right_speed >= 0) ? 1 : -1;

  // 计算当前速度的绝对值
  int16_t left_abs = abs(*left_speed);
  int16_t right_abs = abs(*right_speed);

  // 找到当前最大速度的绝对值
  int16_t current_max = (left_abs > right_abs) ? left_abs : right_abs;

  // 如果当前速度未超过限制，则不需要调整
  if (current_max <= abs_max) {
    return;
  }

  // 计算缩放比例
  float scale_factor = (float)abs_max / current_max;

  // 按比例缩放速度，使用四舍五入提高精度，保持方向不变
  if (left_abs > 0) {
    *left_speed = (int16_t)round(left_sign * left_abs * scale_factor);
  }
  // 原始速度为0时保持0不变
  else {
    *left_speed = 0;
  }

  if (right_abs > 0) {
    *right_speed = (int16_t)round(right_sign * right_abs * scale_factor);
  }
  // 原始速度为0时保持0不变
  else {
    *right_speed = 0;
  }
}

void motion_basic_ctrl(int16_t left_speed, int16_t right_speed) {
#define SELECT_DIR(speed)                                                      \
  (speed > 0 ? MOTOR_FORWARD : (speed < 0 ? MOTOR_BACKWARD : MOTOR_NEUTRAL))
  motor_set_mode(MOTOR_REAR | MOTOR_LEFT, SELECT_DIR(left_speed));
  motor_set_mode(MOTOR_REAR | MOTOR_RIGHT, SELECT_DIR(right_speed));

  current_ls = left_speed;
  current_rs = right_speed;

  limitSpeed(&current_ls, &current_rs, 1000);

  // sprintf(__str_all, "L:%d R:%d", current_ls, current_rs);
  // OLED_ShowString(0, 2, (uint8_t*)__str_all, 16);
  // my_printf("L:%d R:%d\n", current_ls, current_rs);

  motor_set_speed(MOTOR_REAR | MOTOR_LEFT, my_abs(current_ls));
  motor_set_speed(MOTOR_REAR | MOTOR_RIGHT, my_abs(current_rs));
}

void motion_ctrl_with_angle(uint8_t _angle) {
#define BASE_SPEED 100
#define SPEED_RATIO 10

  int angle = _angle - 90;

  int16_t speedl = BASE_SPEED - angle * SPEED_RATIO;
  int16_t speedr = BASE_SPEED + angle * SPEED_RATIO;

  motion_basic_ctrl(speedl, speedr);
}

///////////////////////Algorithm///////////////////////
///////////////////////Algorithm///////////////////////
///////////////////////Algorithm///////////////////////

//////////////////////TURN SECTION/////////////////////

uint8_t adjust_delay = 0;

void test_patrol_pool_with_sys_delta(uint16_t *delta_sys_tick) {
  float delta_second = (float)(*delta_sys_tick) / 1000.0f;
  static float dis = 10.0;
  dis -= 0.1;

  float adjust = pid_update(&hh, dis, delta_second);
  my_log(TAG, "dis %f adjust: %f", dis, adjust);
}

void patrol_pool(float delta_dis) {
  extern int16_t speed;
  float radius_adjustment = pid_update(&hh, delta_dis, -1);

  // my_printf("left speed =%d right speed =%d\n", current_ls, current_rs);

  current_ls = speed - (int16_t)radius_adjustment;
  current_rs = speed + (int16_t)radius_adjustment;

  if (my_abs(radius_adjustment) < 1e-6)
    my_printf("L:%d R:%d adjust:%.1f and ki=%f\n", current_ls, current_rs,
              radius_adjustment, hh.Ki);

  motion_basic_ctrl(current_ls, current_rs);

  mspm0_delay_ms(adjust_delay);

  // if (fabs(delta_dis) > 0.01f || fabs(radius_adjustment) > 0.01f)
  // my_printf("Radius : %f with error: %.1f\n", radius_adjustment, delta_dis);
}

///////////////////////END OF ALGORITHM///////////////////////

void motion_pid_rst(float kp, float ki, float kd, float m) {
  pid_init(&hh, kp, ki, kd, m); // Kp=0.8, Ki=0.2

  sprintf(__str_all, "Kp=%.2f Ki=%.2f ", kp, ki);
  OLED_ShowString(0, 2, __str_all, 16);
  sprintf(__str_all, "Kd=%.2f M=%.2f", kd, m);
  OLED_ShowString(0, 4, __str_all, 16);
}

void motion_init(void) {
  motor_init();
  motion_pid_rst(2, 0, 0, 100);
}